package org.scalpel.mainApps.HHObjects;

import java.util.concurrent.ArrayBlockingQueue;

import org.scalpel.common.HHObject;
import org.scalpel.common.KeyPoint;


/**
 * @author Yvanne Paviet-Salomon
 * @version 1.0
 * 
 * 
 **/


public class Trajectory {
	
	static short NB_POINTS=666 ;
	
	public ArrayBlockingQueue<KeyPoint> fListPoints = new ArrayBlockingQueue<KeyPoint>(NB_POINTS, true);

	KeyPoint start = new KeyPoint(0.250,0.250);
	KeyPoint rotation = new KeyPoint();

	public Trajectory(short Nb_point) {
		NB_POINTS = Nb_point;
	}
	
	public ArrayBlockingQueue<KeyPoint> horizontal()
	{    	
		double i;
		/*for (i=0.250;i<=2.75;i=i+0.5)
		{
			
			if (i==0.75)
			{
				!!angle et systeme de reverse en conflit
				rotation(-1);
				rotation(0.7); // bug
				rotation(0,5); //marche
			}*/

KeyPoint hi = new KeyPoint(2.75,0.250);
	fListPoints.add(hi);
		//}
			
		/**	fListPoints.add(two);
			fListPoints.add(three);
			fListPoints.add(four);*/

		//}     

		
    	fListPoints.add(start);
    	
    	return fListPoints;
    	
	}
	public void rotation(double angle){
		
		rotation.rotate(rotation, angle);
        fListPoints.add(rotation);
	}
	
	public ArrayBlockingQueue<KeyPoint> targetTest2(){
			
			
			 fListPoints = new ArrayBlockingQueue<KeyPoint>(NB_POINTS, true);
			// fListPoints.add(new KeyPoint(0.25,0.25,0,0.01));
		/*	 fListPoints.add(new KeyPoint(0.5,0.25,0,0.01));
			 fListPoints.add(new KeyPoint(0.7,0.25,0,0.01));
			 fListPoints.add(new KeyPoint(1,0.25,0,0.01));*/
			 fListPoints.add(new KeyPoint(1.167,0.265,0,0.01));
			 fListPoints.add(new KeyPoint(0,0,100*Math.PI/180,0.1));
			 fListPoints.add(new KeyPoint(0,0,0.01*Math.PI/180,0.1));
			/* fListPoints.add(new KeyPoint(1.28,0.29,0,0.01));
			 fListPoints.add(new KeyPoint(1.37,0.35,0,0.01));
			 fListPoints.add(new KeyPoint(1.44,0.45,0,0.01));
			 fListPoints.add(new KeyPoint(1.5,0.585,0,0.01));*/
			 fListPoints.add(new KeyPoint(1.508,0.653,0,0.01));
			//fListPoints.add(new KeyPoint(0,0,0.01*Math.PI/180,1));
			 fListPoints.add(new KeyPoint(0,0,20*Math.PI/180,0.1));

			 
			 fListPoints.add(new KeyPoint(1.492,0.659,0,0.01));
			 fListPoints.add(new KeyPoint(1.415,0.681,0,0.01));
			 fListPoints.add(new KeyPoint(1.373,0.682,0,0.01));
			 fListPoints.add(new KeyPoint(1.081,0.58,0,0.01));
			 fListPoints.add(new KeyPoint(0.964,0.475,0,0.01));
			 fListPoints.add(new KeyPoint(0.502,0.284,0,0.01));

			/* fListPoints.add(new KeyPoint(1.49,0.705,0,0.01));
			 fListPoints.add(new KeyPoint(1.475,0.726,0,0.01));
			 fListPoints.add(new KeyPoint(1.433,0.752,0,0.01));
			 fListPoints.add(new KeyPoint(1.413,0.756,0,0.01));
			 fListPoints.add(new KeyPoint(1.395,0.755,0,0.01));
			 fListPoints.add(new KeyPoint(1.358,0.746,0,0.01));
			 fListPoints.add(new KeyPoint(1.3,0.730,0,0.01));
			 fListPoints.add(new KeyPoint(1.254,0.718,0,0.01));
			 fListPoints.add(new KeyPoint(1.209,0.709,0,0.01));
			 fListPoints.add(new KeyPoint(1.18,0.7,0,0.01));
			 fListPoints.add(new KeyPoint(1.1,0.684,0,0.01));
			 fListPoints.add(new KeyPoint(0.977,0.619,0,0.01));
			 fListPoints.add(new KeyPoint(0.859,0.549,0,0.01));
			 fListPoints.add(new KeyPoint(0.77,0.46,0,0.01));
			 fListPoints.add(new KeyPoint(0.427,0.325,0,0.01));*/
				
			return fListPoints;
	}
	
	
	public ArrayBlockingQueue<KeyPoint> targetTest()
	{
 fListPoints = new ArrayBlockingQueue<KeyPoint>(NB_POINTS, true);
 
 fListPoints.add(new KeyPoint(0.74,0.25,0,0.01));
	/*fListPoints.add(new KeyPoint(0, 0,40*Math.PI/180,1));
	fListPoints.add(new KeyPoint(0, 0,60*Math.PI/180,1));
	fListPoints.add(new KeyPoint(0, 0,90*Math.PI/180,1));
	fListPoints.add(new KeyPoint(0, 0,110*Math.PI/180,1));
	fListPoints.add(new KeyPoint(0, 0,130*Math.PI/180,1));*/
	//fListPoints.add(new KeyPoint(0,0,-45*Math.PI/180,1));
	
	/*fListPoints.add(new KeyPoint(0.74,0.728,0,0.01));
	fListPoints.add(new KeyPoint(0, 0,90*Math.PI/180,1));
	fListPoints.add(new KeyPoint(0, 0,90*Math.PI/180,1));

	fListPoints.add(new KeyPoint(0.255, 0.728,0,0.01));//pousser le lingot 
	fListPoints.add(new KeyPoint(0.64, 0.728,0,0.01));
<<<<<<< HEAD
	fListPoints.add(new KeyPoint(0, 0,89*Math.PI/180,0.01));
	//fListPoints.add(new KeyPoint(0.64, 1.95,90*Math.PI/180,0.01));//taper la bouteille
=======

	/*fListPoints.add(new KeyPoint(0, 0,89*Math.PI/180,0.01));
	fListPoints.add(new KeyPoint(0.64, 1.95,90*Math.PI/180,0.01));//taper la bouteille
>>>>>>> 7c6f3c575dc46464f2a0a55ba880254b63273cfe
	
	
	fListPoints.add(new KeyPoint(0.64,1.330,0,0.01));
	//fListPoints.add(new KeyPoint(0, 0,89*Math.PI/180,0.01));
	fListPoints.add(new KeyPoint(0.920,1.330,0,0.01));//1er totem, sleep pour 5,5sec.
	fListPoints.add(new KeyPoint(1.763,1.330,0,0.01));//2e totem, sleep pour 5,5sec.
	fListPoints.add(new KeyPoint(2.3,1.330,0,0.01));
	
	//fListPoints.add(new KeyPoint(0, 0,8*Math.PI/180,0.01)); 
//	fListPoints.add(new KeyPoint(2.3,0.68,0,0.01));
//	fListPoints.add(new KeyPoint(2.3,0.38,0,0.01));
	//fListPoints.add(new KeyPoint(0,0, -45*Math.PI/180,1));
fListPoints.add(start);
	//fListPoints.add(new KeyPoint(0, 0,8*Math.PI/180,0.01));
	
	//fListPoints.add(new KeyPoint(0.115,0.68,0.01*Math.PI/180,0.01));// retourner en poussant les objets.
	//fListPoints.add(start);
	
	
/*
 fListPoints.add(new KeyPoint(0.64, 0.25,0,0.1));
	//fListPoints.add(new KeyPoint(0, 0,90*Math.PI/180,0.1));
	fListPoints.add(new KeyPoint(0.64,  1.05,0,0.1));
	//fListPoints.add(new KeyPoint(0, 0,90*Math.PI/180,0.1));
	
	fListPoints.add(new KeyPoint(0.64,  1.05,0,0.1));	
	fListPoints.add(new KeyPoint(0.115,  1.05,0,0.1));
	fListPoints.add(new KeyPoint(0.64,  1.05,0,0.1));
	//fListPoints.add(new KeyPoint(0, 0,90*Math.PI/180,0.1));
	fListPoints.add(new KeyPoint(0.64, 1.885,0,0.1));
	
    	fListPoints.add(new KeyPoint(1.35, 0.35,90,0.1));
    	fListPoints.add(new KeyPoint(1.35, 1.35,90,0.1));
    	fListPoints.add(new KeyPoint(1.35, 1.35,180,0.1));
    	fListPoints.add(new KeyPoint(0.35, 1.35,180,0.1));
    	fListPoints.add(new KeyPoint(0.35, 1.35,-90,0.1));
    	fListPoints.add(new KeyPoint(0.35, 0.35,-90,0.1));
    	fListPoints.add(new KeyPoint(0.35, 0.35,0,0.1));*/
    	return fListPoints;
    		}
	public ArrayBlockingQueue<KeyPoint> retour_debut()
	{
    	fListPoints.add(new KeyPoint(1.35, 0.35,90,0.1));
    	fListPoints.add(new KeyPoint(1.35, 1.35,90,0.1));
    	fListPoints.add(new KeyPoint(1.35, 1.35,180,0.1));
    	fListPoints.add(new KeyPoint(0.35, 1.35,180,0.1));
    	fListPoints.add(new KeyPoint(0.35, 1.35,-90,0.1));
    	fListPoints.add(new KeyPoint(0.35, 0.35,-90,0.1));
    	fListPoints.add(new KeyPoint(0.35, 0.35,0,0.1));
    	
    	return fListPoints;
	}

	

	
	
}
